/*
 * @Description: 显示模块任务管理文件 
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:06:34
 * @LastEditTime: 2021-11-30 21:56:42
 * @LastEditors: Sang Hao
 */

#include "glog/logging.h"
#include "lidar_slam/global_defination/global_defination.h"
#include "lidar_slam/mapping/viewer/viewer_flow.hpp"

#include "lidar_slam/models/segmentation/ransac_cluster_segment.hpp"

namespace lidar_slam {

/* public 公有函数，保留的对外接口 */
/**
 * @description: ViewerFlow的构造函数
 * @param  {*}
 * @return {*}
 */
ViewerFlow::ViewerFlow(ros::NodeHandle& nh) {
	//subscriber
	cloud_sub_ptr_ = std::make_shared<CloudSubscriber>(nh, "synced_cloud", 100000);
	key_frame_sub_ptr_ = std::make_shared<KeyFrameSubscriber>(nh, "/key_frame", 100000);
	transformed_odom_sub_ptr_ = std::make_shared<OdometrySubscriber>(nh, "/transformed_odom", 100000);
	optimized_key_frames_sub_ptr_ = std::make_shared<KeyFramesSubscriber>(nh, "/optimized_key_frames", 100000);

	/* publisher */
	optimized_odom_pub_ptr_ = std::make_shared<OdometryPublisher>(nh, "/optimized_odom", "/map", "/lidar", 100);
	current_scan_pub_ptr_ = std::make_shared<CloudPublisher>(nh, "/current_scan", "/map", 100);
	global_map_pub_ptr_ = std::make_shared<CloudPublisher>(nh, "/global_map", "/map", 100);
	local_map_pub_ptr_ = std::make_shared<CloudPublisher>(nh, "/local_map", "/map", 100);

	/* segmentation */
	seged_map_pub_ptr_ = std::make_shared<CloudPublisher>(nh, "/seged_map", "/map", 100);
	/* viewer */
	/* 构造时即完成了初始化：InitWithConfig */
	viewer_ptr_ = std::make_shared<Viewer>();
}

/**
 * @description: 对外运行总接口
 * @param  {*}
 * @return {*}
 */
bool ViewerFlow::Run() {
	if (!ReadData()) {
		return false;
	}

	while (HasData()) {
		if (ValidData()) {
			viewer_ptr_->UpdateWithNewKeyFrame(key_frame_buff_, current_transformed_odom_, current_cloud_data_);
			PublishLocalData();
		}
	}

	if (optimized_key_frames_.size() > 0) {
		viewer_ptr_->UpdateWithOptimizedKeyFrames(optimized_key_frames_);
		PublishGlobalData();
	}

	return true;
}

/**
 * @description: 保存地图的服务
 * @param  {*}
 * @return {*}
 */
bool ViewerFlow::SaveMap() {
	return viewer_ptr_->SaveMap();

}

bool ViewerFlow::PublishGlobalSegData() {
	CloudData::CLOUD_PTR global_map_ptr(new CloudData::CLOUD());
	CloudRGBData::CLOUD_PTR global_map_seg_ptr(new CloudRGBData::CLOUD());
	
	viewer_ptr_->GetGlobalMap(global_map_ptr);
	
	viewer_ptr_->SegMap(global_map_ptr, global_map_seg_ptr);

	/* 保存地图 */
	std::string seg_map_file_path = WORK_SPACE_PATH + "/slam_data/map/seged_map.pcd";
	std::cout << "正在保存seged_map, 共 " 
			  << global_map_seg_ptr->points.size() 
			  << " 个点！"  << std::endl;
	pcl::io::savePCDFile(seg_map_file_path, *global_map_seg_ptr);

	std::cout << "已经保存地图seged_map，地址是：" << seg_map_file_path << std::endl; 
	seged_map_pub_ptr_->Publish(global_map_seg_ptr);
	std::cout << "发布了global_seg_data, 共 " 
			  << global_map_seg_ptr->points.size() 
			  << " 个点！" << std::endl;

	return true;
}

/* private 私有函数 */
/**
 * @description: 读数据的函数，为何总是出现异常段错误
 * @param  {*}
 * @return {*}
 */
bool ViewerFlow::ReadData() {
	cloud_sub_ptr_->ParseData(cloud_data_buff_);
	transformed_odom_sub_ptr_->ParseData(transformed_odom_buff_);
	key_frame_sub_ptr_->ParseData(key_frame_buff_);
	optimized_key_frames_sub_ptr_->ParseData(optimized_key_frames_);

	return true;
}

/**
 * @description: 判断是否有数据
 * @param  {*}
 * @return {*}
 */
bool ViewerFlow::HasData() {
	if (cloud_data_buff_.size() == 0) {
		return false;
	}
	if (transformed_odom_buff_.size() == 0) {
		return false;
	}
	
	return true;
}

/**
 * @description: 判断数据是否有效，有效后则从buff中取出一个数据。
 * 				该函数前已经完成了HasData()的判断
 * @param  {*}
 * @return {*}
 */
bool ViewerFlow::ValidData() {
	current_cloud_data_ = cloud_data_buff_.front();
	current_transformed_odom_ = transformed_odom_buff_.front();

	cloud_data_buff_.pop_front();
	transformed_odom_buff_.pop_front();

	return true;
}

/**
 * @description: 发布全局点云数据
 * @param  {*}
 * @return {*}
 */
bool ViewerFlow::PublishGlobalData() {
	if (viewer_ptr_->HasNewGlobalMap() && global_map_pub_ptr_->HasSubscribers()) {
		CloudData::CLOUD_PTR cloud_ptr(new CloudData::CLOUD());
		viewer_ptr_->GetGlobalMap(cloud_ptr);
		global_map_pub_ptr_->Publish(cloud_ptr);
	}

	return true;
}

/**
 * @description: 发布局部点云数据
 * @param  {*}
 * @return {*}
 */
bool ViewerFlow::PublishLocalData() {
	optimized_odom_pub_ptr_->Publish(viewer_ptr_->GetCurrentPose());
	current_scan_pub_ptr_->Publish(viewer_ptr_->GetCurrentScan());

	if (viewer_ptr_->HasNewLocalMap() && local_map_pub_ptr_->HasSubscribers()) {
		CloudData::CLOUD_PTR cloud_ptr(new CloudData::CLOUD());
		viewer_ptr_->GetLocalMap(cloud_ptr);
		local_map_pub_ptr_->Publish(cloud_ptr);
	}

	return true;
}

}